#include <algorithm>
#include <iostream>
#include <atomic>
#include <array>
#include "CanDriver.hpp"

using sockcanpp::CanDriver;
using sockcanpp::CanMessage;
using std::atomic;
using std::array;

atomic<array<uint8_t, 8>> amp_data;

can_frame control_frame;

void sendFeedback1(CanDriver& canDriver) {
    while (true) {
        array<uint8_t, 8> data = amp_data.load();
        std::copy_n(data.begin(), 8, control_frame.data);
        canDriver.sendMessage(CanMessage(control_frame));
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

void getUserInput() {
    int16_t input;
    while (true) {
        std::cin >> input;
        if (input >= -INT16_MAX / 2 && input <= INT16_MAX / 2) {
            array<uint8_t, 8> new_data = {};
            new_data[0] = input >> 8;
            new_data[1] = input & 0xff;

            amp_data.store(new_data);

        } else {
            std::cout << "Invalid input. " << std::endl;
        }
    }
}

int main() {
    control_frame.can_id = 0x200;
    control_frame.len = 8;
    array<uint8_t, 8> init_data = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    std::copy_n(init_data.begin(), 8, control_frame.data);

    CanDriver canDriver("can0", CAN_RAW);

    std::thread sender(sendFeedback1, std::ref(canDriver));
    std::thread inputHandler(getUserInput);

    sender.join();
    inputHandler.join();

    return 0;
}
